/*
 * Author: Ken Beck
 *
 * Class for constructing a grid representation of an environment.
 *
 */

#include "Grid.h"

#include <fstream>

ostream& operator<<(ostream& out, const GridCoord& x) {
  out << x.wPos << "; " << x.cPos << "; " << x.cVal;
  return out;
}

ostream& operator<<(ostream& out, const DCoord& c) {
   out << "(" << c.x << "," << c.y << ")";
   return out;
}

Coord Grid::get_worldCoord(GridCoord *at){
   return Coord(at->wPos.x*meters_per_pixel,at->wPos.y*meters_per_pixel);
}

Grid::Grid(vector<vector<GridCoord> > grid){
   this->grid = grid;
}

DCoord Grid::workCoords_to_cCoords(DCoord w){
   double x = (double)w.x/cSpace_pixels;
   double y = (double)w.y/cSpace_pixels;
   
   //cout << w << ";" << DCoord((x<0?ceil(x-0.5):floor(x+0.5)),(y<0?ceil(y-0.5):floor(y+0.5))) << endl;
   
   return DCoord((int)(x<0?ceil(x-0.5):floor(x+0.5)),(int)(y<0?ceil(y-0.5):floor(y+0.5)));
}

DCoord Grid::cCoords_to_workCoords(DCoord c){
   return DCoord(c.x*cSpace_pixels,c.y*cSpace_pixels);
}

Grid::Grid(vector<vector<GridCoord> > grid,double meters_per_pixel,double box_size){
   this->cSpace_pixels = (int)(1/meters_per_pixel*box_size); // Pixels per bot
   this->meters_per_pixel = meters_per_pixel;
   this->grid = grid;
 
   int startX = -(int)grid.size()/2 + (int)grid.size()/2 % cSpace_pixels;
   int startY = -(int)grid[0].size()/2 + (int)grid[0].size()/2 % cSpace_pixels;
 
   int x,y;

   for(x=startX;x<-startX;x+=cSpace_pixels){
      for(y=startY;y<-startY;y+=cSpace_pixels){
         GridCoord *at = get_gridCoord(DCoord(x,y));
         
         DCoord cPos = workCoords_to_cCoords(at->wPos);
       
         int i,j;
         bool allOne=false;
         
      //findObst:
         for(i=-cSpace_pixels/2;i<cSpace_pixels/2;i++){
            for(j=-cSpace_pixels/2;j<cSpace_pixels/2;j++){
               at = get_gridCoord(DCoord(x+i,y+j));
               if( at != 0 && at->wVal == 1 ){
                  allOne=true;
                  i=cSpace_pixels;
                  break;
               }
            }
         }
         
         for(i=-cSpace_pixels/2;i<cSpace_pixels/2;i++){
            for(j=-cSpace_pixels/2;j<cSpace_pixels/2;j++){
               at = get_gridCoord(DCoord(x+i,y+j));

               if( at != 0 ){
                  at->cPos = cPos;
                  if( allOne ){
                     at->cVal = 1;
                  }else{
                     at->cVal = 0;
                  }
               }
            }
         }
      }
   }
}

GridCoord *Grid::operator[](const Coord at){
   return this->get_gridCoord(at);
}

GridCoord *Grid::operator[](const DCoord at){
   return this->get_gridCoord(at);
}

GridCoord *Grid::operator()(int x,int y){
   return this->get_gridCoord(DCoord(x,y));
}

double Grid::dist_between(const GridCoord *from,const GridCoord *to){
   return sqrt(pow(fabs(to->cPos.x - from->cPos.x),2)+pow(fabs(to->cPos.y - from->cPos.y),2));
}

GridCoord *Grid::get_gridCoordFromCSpace(const DCoord at){
   return get_gridCoord(cCoords_to_workCoords(at));
}

GridCoord *Grid::get_gridCoord(const Coord at){
   return get_gridCoord(DCoord((int)(at.x/meters_per_pixel),(int)(at.y/meters_per_pixel)));
}

GridCoord *Grid::get_gridCoord(const DCoord at){
   unsigned int x,y;

   x = at.x+grid.size()/2;
   y = grid[0].size()/2-at.y;
   
   if( x >= grid.size() || y >= grid[0].size() ){
      return 0;
   }else{
      return &grid[x][y];
   }
}

vector<vector<GridCoord> > Grid::parse_gridFile(char *filename,int width,int height){
   vector<vector<GridCoord> > grid(width,vector<GridCoord>(height,GridCoord(DCoord(0,0),0.0)));
   ifstream ptsStream;
   
   ptsStream.open(filename);

   char val;
   int x=0,y=0;
   while( ptsStream.read(&val,1) ){
      if( x == width ){
         x=0;y++;
      }

      grid[x][y] = GridCoord(DCoord(x-width/2,height/2-y),val==-1?1.0:0.0);
      x++;
   }
   
   return grid;
}

list<GridCoord*> Grid::get_neighbors(const GridCoord *from){
   list<GridCoord*> adj;
   GridCoord *which;
   
   // Loop over 8 possible neighbors
   for(int i=-1;i<=1;i++){
      for(int j=-1;j<=1;j++){
         if( i == 0 && j == 0 ) continue;
         DCoord at(from->cPos.x+i,from->cPos.y+j);
         if( (which = get_gridCoordFromCSpace(at)) != 0 ){
            adj.push_back(which);
         }
      }
   }
   return adj;
}


list<GridCoord*> Grid::vertex_reduce(list<GridCoord*> path){
   list<GridCoord*> optimized;
   
   while( path.size() > 1){
      GridCoord *at = path.front();
      path.pop_front();
      optimized.push_back(at);
      
      GridCoord *top = path.front();
      
      bool xDiff = (at->cPos.x != top->cPos.x);
      bool yDiff = (at->cPos.y != top->cPos.y);
     
      bool xDiff2=!xDiff,yDiff2=!yDiff;
      
      do{
         top = path.front();
         path.pop_front();
         
         if( !path.empty() ){
            xDiff2 = (at->cPos.x != path.front()->cPos.x);
            yDiff2 = (at->cPos.y != path.front()->cPos.y);
         }else{
            xDiff2 = !xDiff;
            yDiff2 = !yDiff;
         }
      }while(xDiff == xDiff2 && yDiff == yDiff2);
      
      optimized.push_back(top);
   }
   
   return optimized;
}
